#include "pch.h"

#include "BipedalMecha.h"
#include "BasicShapes.h"

static BasicShapes^ _bipedalMechaBasicShapes = nullptr;

////////////////////////////////////////////////////////////////////////////////////////////////

BipedalMecha::BipedalMecha()
{
	this->InitializeBipedalSceneGraph();
}

BipedalMecha::~BipedalMecha()
{
	// Call destructors of all resources allocated.
}

////////////////////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetPelvis(SceneNode* node)
{
	this->centerOfMass->Add(node);
}

void BipedalMecha::SetLowerTorso(SceneNode* node)
{
	this->jointLowerTorso->Add(node);
}

void BipedalMecha::SetTorso(SceneNode* node)
{
	this->jointTorso->Add(node);
}

void BipedalMecha::SetChest(SceneNode* node)
{
	this->jointChest->Add(node);
}


void BipedalMecha::SetNeck(SceneNode* node)
{
	this->jointNeck->Add(node);
}

void BipedalMecha::SetHead(SceneNode* node)
{
	this->jointHead->Add(node);
}


void BipedalMecha::SetLeftChest(SceneNode* node)
{
	this->jointLeftChest->Add(node);
}

//////////////////////////////////////////////////////////////////////////////////
// Left Arm
void BipedalMecha::SetLeftShoulder(SceneNode* node)
{
	this->jointLeftShoulder->Add(node);
}

void BipedalMecha::SetLeftUpperArm(SceneNode* node)
{
	this->jointLeftUpperArm->Add(node);
}

void BipedalMecha::SetLeftForearm(SceneNode* node)
{
	this->jointLeftElbow->Add(node);
}

void BipedalMecha::SetLeftHand(SceneNode* node)
{
	this->jointLeftWrist->Add(node);
}

void BipedalMecha::SetLeftFingers(SceneNode* node)
{
	this->jointLeftFingers->Add(node);
}

//////////////////////////////////////////////////////////////////////////////////
// Right Arm
void BipedalMecha::SetRightChest(SceneNode* node)
{
	this->jointRightChest->Add(node);
}

void BipedalMecha::SetRightShoulder(SceneNode* node)
{
	this->jointRightShoulder->Add(node);
}

void BipedalMecha::SetRightUpperArm(SceneNode* node)
{
	this->jointRightUpperArm->Add(node);
}

void BipedalMecha::SetRightForearm(SceneNode* node)
{
	this->jointRightElbow->Add(node);
}

void BipedalMecha::SetRightHand(SceneNode* node)
{
	this->jointRightWrist->Add(node);
}

void BipedalMecha::SetRightFingers(SceneNode* node)
{
	this->jointRightFingers->Add(node);
}

// Left Leg

void BipedalMecha::SetLeftPelvis(SceneNode* node)
{
	this->jointLeftPelvis->Add(node);
}

void BipedalMecha::SetLeftThigh(SceneNode* node)
{
	this->jointLeftHip->Add(node);
}

void BipedalMecha::SetLeftShin(SceneNode* node)
{
	this->jointLeftKnee->Add(node);
}

void BipedalMecha::SetLeftFoot(SceneNode* node)
{
	this->jointLeftAnkle->Add(node);
}

void BipedalMecha::SetLeftToe(SceneNode* node)
{
	this->jointLeftToes->Add(node);
}

// Left Leg

void BipedalMecha::SetRightPelvis(SceneNode* node)
{
	this->jointRightPelvis->Add(node);
}

void BipedalMecha::SetRightThigh(SceneNode* node)
{
	this->jointRightHip->Add(node);
}

void BipedalMecha::SetRightShin(SceneNode* node)
{
	this->jointRightKnee->Add(node);
}

void BipedalMecha::SetRightFoot(SceneNode* node)
{
	this->jointRightAnkle->Add(node);
}

void BipedalMecha::SetRightToe(SceneNode* node)
{
	this->jointRightToes->Add(node);
}




////////////////////////////////////////////////////////////////////////////////////////////////
// Sets Basic Dimensions to quickly get a Mech ready for use.
void BipedalMecha::SetBasicMechaDimensions(
		float NeckHeight, float HeadHeight,
		float LowerTorsoLength, float TorsoLength, float ChestHeight, 
		float ChestWidth,
		float ShoulderWidth, float UpperArmLength, float ForearmLength, float HandLength, float FingersLength,
		float PelvisWidth, float ThighLength, float ShinLength, float FootHeight, float FootLength, float ToeLength,
		bool CreateWithBoxGeometry,

		ID3D11Device *d3dDevice,
		
		ShapeLibrary* ShapeLibrary,
		int TextureID,
		int ShaderID,
		ModelLibrary* ModelLibrary)
{
	this->SetRelativeCOM(0, ThighLength + ShinLength + FootHeight, 0);

	this->SetNeckSize(0, NeckHeight, 0);
	this->SetHeadSize(0, HeadHeight, 0);

	this->SetLowerTorsoSize(0, LowerTorsoLength, 0);
	this->SetTorsoSize(0, TorsoLength, 0);
	this->SetChestSize(0, ChestHeight, 0);

	///////////////////////////////////////////////////////////////////////////////////////////
	// The Arms

	// Left:
	this->SetLeftChestSize(-1.0f * ChestWidth, 0.0f, 0.0f);
	this->SetLeftShoulderSize(-1.0f * ShoulderWidth, 0.0f, 0.0f);
	this->SetLeftUpperArmSize(0, -1.0f * UpperArmLength, 0);
	this->SetLeftForearmSize(0, -1.0f * ForearmLength, 0);
	this->SetLeftHandSize(0, -1.0f * HandLength, 0);
	this->SetLeftFingersSize(0, -1.0f * FingersLength, 0);

	// Right:
	this->SetRightChestSize(ChestWidth, 0.0f, 0.0f);
	this->SetRightShoulderSize(ShoulderWidth, 0.0f, 0.0f);
	this->SetRightUpperArmSize(0, -1.0f * UpperArmLength, 0);
	this->SetRightForearmSize(0, -1.0f * ForearmLength, 0);
	this->SetRightHandSize(0, -1.0f * HandLength, 0);
	this->SetRightFingersSize(0, -1.0f * FingersLength, 0);

	////////////////////////////////////////////////////////////////////////////////////////////
	// The Legs

	// Left:
	this->SetLeftPelvisSize(-1.0f * PelvisWidth, 0, 0);
	this->SetLeftThighSize(0, -1.0f * ThighLength, 0);
	this->SetLeftShinSize(0, -1.0f * ShinLength, 0);
	this->SetLeftFootSize(0, -1.0f * FootHeight, FootLength);
	this->SetLeftToesSize(0, 0, ToeLength);

	// Right:
	this->SetRightPelvisSize(PelvisWidth, 0, 0);
	this->SetRightThighSize(0, -1.0f * ThighLength, 0);
	this->SetRightShinSize(0, -1.0f * ShinLength, 0);
	this->SetRightFootSize(0, -1.0f * FootHeight, FootLength);
	this->SetRightToesSize(0, 0, ToeLength);

	if (CreateWithBoxGeometry)
	{
		this->SetNeck(
			this->CreateSimpleBox(
				NeckHeight / 4, NeckHeight, NeckHeight / 4, 0, NeckHeight / 4, NeckHeight / 4,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		
		this->SetPelvis(
			this->CreateSimpleBox(
				PelvisWidth / 4, 0, PelvisWidth / 4, PelvisWidth / 4, PelvisWidth / 2, PelvisWidth / 4,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		//////////////////////////////////////////////////////////////////////////////////////////////
		//The torso
		this->SetLowerTorso(
			this->CreateSimpleBox(
			ChestWidth / 3, LowerTorsoLength, ChestWidth / 3, 0, ChestWidth / 5, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetTorso(
			this->CreateSimpleBox(
			ChestWidth / 2.5, TorsoLength, ChestWidth / 2.5, 0, ChestWidth / 4, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetChest(
			this->CreateSimpleBox(
			ChestWidth / 4, ChestHeight, ChestWidth / 4, 0, ChestWidth / 1.5f, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		//////////////////////////////////////////////////////////////////////////////////////////////
		// left Arm
		this->SetLeftChest(
			this->CreateSimpleBox(
			ChestWidth, 0, 0, ChestHeight / 2, ChestWidth / 2, 0,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetLeftShoulder(
			this->CreateSimpleBox(
				ShoulderWidth, 0, 0, ChestHeight / 2.25f, ChestWidth / 2.5, 0,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetLeftUpperArm(
			this->CreateSimpleBox(
			ShoulderWidth / 2.2f, 0, ShoulderWidth / 2.2f, UpperArmLength, ShoulderWidth / 2.2f, ShoulderWidth / 2.2f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetLeftForearm(
			this->CreateSimpleBox(
			ShoulderWidth / 2.5f, 0, ShoulderWidth / 2.5f, ForearmLength, ShoulderWidth / 2.5f, ShoulderWidth / 2.5f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetLeftHand(
			this->CreateSimpleBox(
			ShoulderWidth / 3.5f, 0, ShoulderWidth / 3.5f, HandLength, ShoulderWidth / 3, ShoulderWidth / 3,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetLeftFingers(
			this->CreateSimpleBox(
			ShoulderWidth / 3.5f, 0, ShoulderWidth / 3.5f, FingersLength, ShoulderWidth / 3.2f, ShoulderWidth / 3.2f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		//////////////////////////////////////////////////////////////////////////////////////////////
		// Right Arm
		this->SetRightChest(
			this->CreateSimpleBox(
			0, 0, ChestWidth, ChestHeight / 2, ChestWidth / 2, 0,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetRightShoulder(
			this->CreateSimpleBox(
				0, 0, ShoulderWidth, ChestHeight / 2.25f, ChestWidth / 2.5, 0,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetRightUpperArm(
			this->CreateSimpleBox(
			ShoulderWidth / 2.2f, 0, ShoulderWidth / 2.2f, UpperArmLength, ShoulderWidth / 2.2f, ShoulderWidth / 2.2f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetRightForearm(
			this->CreateSimpleBox(
			ShoulderWidth / 2.5f, 0, ShoulderWidth / 2.5f, ForearmLength, ShoulderWidth / 2.5f, ShoulderWidth / 2.5f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetRightHand(
			this->CreateSimpleBox(
			ShoulderWidth / 3.5f, 0, ShoulderWidth / 3.5f, HandLength, ShoulderWidth / 3, ShoulderWidth / 3,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));

		this->SetRightFingers(
			this->CreateSimpleBox(
			ShoulderWidth / 3.5f, 0, ShoulderWidth / 3.5f, FingersLength, ShoulderWidth / 3.2f, ShoulderWidth / 3.2f,
					d3dDevice,
					ShapeLibrary,
					TextureID,
					ShaderID,
					ModelLibrary));



		//////////////////////////////////////////////////////////////////////////////////////////////
		// left leg

		this->SetLeftPelvis(
			this->CreateSimpleBox(
				PelvisWidth, 0, 0, PelvisWidth / 4, PelvisWidth / 4, PelvisWidth / 4,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetLeftThigh(
			this->CreateSimpleBox(
				PelvisWidth / 2, 0, PelvisWidth / 4, ThighLength, PelvisWidth / 4.5, PelvisWidth / 4.5,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetLeftShin(
			this->CreateSimpleBox(
				PelvisWidth / 2.5f, 0, PelvisWidth / 5, ShinLength, PelvisWidth / 5, PelvisWidth / 5,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetLeftFoot(
			this->CreateSimpleBox(
				PelvisWidth / 4, 0, PelvisWidth / 6, FootHeight, FootLength, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetLeftToe(
			this->CreateSimpleBox(
				PelvisWidth / 4, FootHeight / 4, PelvisWidth / 6, 0, ToeLength, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		//////////////////////////////////////////////////////////////////////////////////////////////
		// Right leg

		this->SetRightPelvis(
			this->CreateSimpleBox(
				0, 0, PelvisWidth, PelvisWidth / 4, PelvisWidth / 4, PelvisWidth / 4,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetRightThigh(
			this->CreateSimpleBox(
				PelvisWidth / 4, 0, PelvisWidth / 2, ThighLength, PelvisWidth / 4.5, PelvisWidth / 4.5,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetRightShin(
			this->CreateSimpleBox(
				PelvisWidth / 5, 0, PelvisWidth / 2.5, ShinLength, PelvisWidth / 5, PelvisWidth / 5,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetRightFoot(
			this->CreateSimpleBox(
				PelvisWidth / 6, 0, PelvisWidth / 4, FootHeight, FootLength, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));

		this->SetRightToe(
			this->CreateSimpleBox(
				PelvisWidth / 6, FootHeight / 4, PelvisWidth / 4, 0, ToeLength, 0,
				d3dDevice,
				ShapeLibrary,
				TextureID,
				ShaderID,
				ModelLibrary));
	}
}


// Set the transformation from the COM to the end of the pelvis where the
// child joints connect to.
void BipedalMecha::SetPelvisSize(float x, float y, float z)
{
	pelvis->localTrandormation = 
		translation(x,y,z);
}

// Sets the transformation from the top of the pelvis to the top of
// the lower torso.
void BipedalMecha::SetLowerTorsoSize(float x, float y, float z)
{
	lowerTorso->localTrandormation = 
		translation(x,y,z);
}

// Sets the transformation from the top of the lower torso to the top of
// the torso.
void BipedalMecha::SetTorsoSize(float x, float y, float z)
{
	torso->localTrandormation = 
		translation(x,y,z);
}

// Sets the transformation from the top of the torso to the top of
// the chest.
void BipedalMecha::SetChestSize(float x, float y, float z)
{
	chest->localTrandormation = 
		translation(x,y,z);
}

// Sets the transformation from the top of the chest to the top of
// the neck.
void BipedalMecha::SetNeckSize(float x, float y, float z)
{
	neck->localTrandormation = 
		translation(x,y,z);
}

// Sets the transformation from the top of the neck to the top of
// the head.
void BipedalMecha::SetHeadSize(float x, float y, float z)
{
	head->localTrandormation = 
		translation(x,y,z);
}

///////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetLeftChestSize(float x, float y, float z)
{
	leftChest->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftShoulderSize(float x, float y, float z)
{
	leftShoulder->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftUpperArmSize(float x, float y, float z)
{
	leftUpperArm->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftForearmSize(float x, float y, float z)
{
	leftForearm->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftHandSize(float x, float y, float z)
{
	leftHand->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftFingersSize(float x, float y, float z)
{
	leftFingers->localTrandormation = 
		translation(x,y,z);
}

///////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetRightChestSize(float x, float y, float z)
{
	rightChest->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightShoulderSize(float x, float y, float z)
{
	rightShoulder->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightUpperArmSize(float x, float y, float z)
{
	rightUpperArm->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightForearmSize(float x, float y, float z)
{
	rightForearm->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightHandSize(float x, float y, float z)
{
	rightHand->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightFingersSize(float x, float y, float z)
{
	rightFingers->localTrandormation = 
		translation(x,y,z);
}

///////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetLeftPelvisSize(float x, float y, float z)
{
	leftPelvis->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftThighSize(float x, float y, float z)
{
	leftThigh->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftShinSize(float x, float y, float z)
{
	leftShin->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftFootSize(float x, float y, float z)
{
	leftFoot->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetLeftToesSize(float x, float y, float z)
{
	leftToes->localTrandormation = 
		translation(x,y,z);
}

///////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetRightPelvisSize(float x, float y, float z)
{
	rightPelvis->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightThighSize(float x, float y, float z)
{
	rightThigh->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightShinSize(float x, float y, float z)
{
	rightShin->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightFootSize(float x, float y, float z)
{
	rightFoot->localTrandormation = 
		translation(x,y,z);
}

void BipedalMecha::SetRightToesSize(float x, float y, float z)
{
	rightToes->localTrandormation = 
		translation(x,y,z);
}

///////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::SetRelativeCOM(float x, float y, float z)
{
	centerOfMass->localTrandormation = translation(x,y,z);
}

////////////////////////////////////////////////////////////////////////////////////

// Rotates the lower torso about the Euler X,Z,Y
void BipedalMecha::RotateLowerTorso(float x, float y, float z)
{
	jointLowerTorso->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

// Rotates the torso about the Euler X,Z,Y
void BipedalMecha::RotateTorso(float x, float y, float z)
{
	jointTorso->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

// Rotates the chest about the Euler X,Z,Y
void BipedalMecha::RotateChest(float x, float y, float z)
{
	jointChest->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

///////////////////////////////////////////////////////////////////////////////////

// Rotates the neck about the Euler X,Z,Y
void BipedalMecha::RotateNeck(float x, float y, float z)
{
	jointNeck->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

// Rotates the head about the Euler X,Z,Y
void BipedalMecha::RotateHead(float x, float y, float z)
{
	jointHead->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}
///////////////////////////////////////////////////////////////////////////////////

// Sets the Y rotation of the Left Joint
void BipedalMecha::RotateLeftPelvisY(float y)
{
	jointLeftPelvis->localTrandormation = rotationY(y);
}

// Sets the rotation of the left hip (socket joint) x, z, y rotation.
// euler x, z, y.
void BipedalMecha::RotateLeftHip(float x, float y, float z)
{
	jointLeftHip->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

// Rotate the left knee
void BipedalMecha::RotateLeftKnee(float x)
{
	jointLeftKnee->localTrandormation = rotationX(x);
}

// Rotate the left ankle by the x, z axis.
void BipedalMecha::RotateLeftAnkle(float x, float z)
{
	jointLeftAnkle->localTrandormation = 
		mul(rotationX(x), rotationZ(z));
}

// Rotate the toes about the x axis.
void BipedalMecha::RotateLeftToe(float x)
{
	jointLeftToes->localTrandormation = rotationX(x);
}

////////////////////////////////////////////////////////////////////////////////

// Sets the Y rotation of the Right Joint
void BipedalMecha::RotateRightPelvisY(float y)
{
	jointRightPelvis->localTrandormation = rotationY(y);
}

// Sets the rotation of the Right hip (socket joint) x, z, y rotation.
// euler x, z, y.
void BipedalMecha::RotateRightHip(float x, float y, float z)
{
	jointRightHip->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

// Rotate the Right knee
void BipedalMecha::RotateRightKnee(float x)
{
	jointRightKnee->localTrandormation = rotationX(x);
}

// Rotate the Right ankle by the x, z axis.
void BipedalMecha::RotateRightAnkle(float x, float z)
{
	jointRightAnkle->localTrandormation = 
		mul(rotationX(x), rotationZ(z));
}

// Rotate the toes about the x axis.
void BipedalMecha::RotateRightToe(float x)
{
	jointRightToes->localTrandormation = rotationX(x);
}

////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::RotateLeftChestY(float y)
{
	jointLeftChest->localTrandormation = rotationY(y);
}

void BipedalMecha::RotateLeftShoulder(float x, float y, float z)
{
	jointLeftShoulder->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

void BipedalMecha::RotateLeftElbow(float x, float y)
{
	jointLeftElbow->localTrandormation =
		mul(rotationX(x), rotationZ(y));
}

void BipedalMecha::RotateLeftWrist(float x, float z)
{
	jointLeftWrist->localTrandormation =
		mul(rotationX(x), rotationZ(z));
}

void BipedalMecha::RotateLeftFingers(float x)
{
	jointLeftElbow->localTrandormation = rotationX(x);
}

////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::RotateRightChestY(float y)
{
	jointRightChest->localTrandormation = rotationY(y);
}

void BipedalMecha::RotateRightShoulder(float x, float y, float z)
{
	jointRightShoulder->localTrandormation =
		mul(mul(rotationX(x), rotationZ(z)), rotationY(y));
}

void BipedalMecha::RotateRightElbow(float x, float y)
{
	jointRightElbow->localTrandormation =
		mul(rotationX(x), rotationZ(y));
}

void BipedalMecha::RotateRightWrist(float x, float z)
{
	jointRightWrist->localTrandormation =
		mul(rotationX(x), rotationZ(z));
}

void BipedalMecha::RotateRightFingers(float x)
{
	jointRightElbow->localTrandormation = rotationX(x);
}

////////////////////////////////////////////////////////////////////////////////

void BipedalMecha::InitializeBipedalSceneGraph()
{
	centerOfMass = new SceneNode();

	jointLowerTorso = new SceneNode();
	jointTorso = new SceneNode();
	jointChest = new SceneNode();

	jointNeck = new SceneNode();
	jointHead = new SceneNode();

	jointLeftChest = new SceneNode();
	jointLeftShoulder = new SceneNode();
	jointLeftUpperArm = new SceneNode();
	jointLeftElbow = new SceneNode();
	jointLeftWrist = new SceneNode();
	jointLeftFingers = new SceneNode();

	jointRightChest = new SceneNode();
	jointRightShoulder = new SceneNode();
	jointRightUpperArm = new SceneNode();
	jointRightElbow = new SceneNode();
	jointRightWrist = new SceneNode();
	jointRightFingers = new SceneNode();

	jointLeftPelvis = new SceneNode();
	jointLeftHip = new SceneNode();
	jointLeftKnee = new SceneNode();
	jointLeftAnkle = new SceneNode();
	jointLeftToes = new SceneNode();

	jointRightPelvis = new SceneNode();
	jointRightHip = new SceneNode();
	jointRightKnee = new SceneNode();
	jointRightAnkle = new SceneNode();
	jointRightToes = new SceneNode();

	pelvis = new SceneNode();
	lowerTorso = new SceneNode();
	torso = new SceneNode();
	chest = new SceneNode();

	neck = new SceneNode();

	head = new SceneNode();
	
	leftChest = new SceneNode();
	leftShoulder = new SceneNode();
	leftUpperArm = new SceneNode();
	leftForearm = new SceneNode();
	leftHand = new SceneNode();
	leftFingers = new SceneNode();

	rightChest = new SceneNode();
	rightShoulder = new SceneNode();
	rightUpperArm = new SceneNode();
	rightForearm = new SceneNode();
	rightHand = new SceneNode();
	rightFingers = new SceneNode();

	leftPelvis = new SceneNode();
	leftThigh = new SceneNode();
	leftShin = new SceneNode();
	leftFoot = new SceneNode();
	leftToes = new SceneNode();

	rightPelvis = new SceneNode();
	rightThigh = new SceneNode();
	rightShin = new SceneNode();
	rightFoot = new SceneNode();
	rightToes = new SceneNode();

	// Create the Heirachy
	this->Add(centerOfMass);

	centerOfMass->Add(pelvis);

	// Left leg
	pelvis->Add(jointLeftPelvis);
	jointLeftPelvis->Add(leftPelvis);
	leftPelvis->Add(jointLeftHip);
	jointLeftHip->Add(leftThigh);
	leftThigh->Add(jointLeftKnee);
	jointLeftKnee->Add(leftShin);
	leftShin->Add(jointLeftAnkle);
	jointLeftAnkle->Add(leftFoot);
	leftFoot->Add(jointLeftToes);
	jointLeftToes->Add(leftToes);

	// right leg
	pelvis->Add(jointRightPelvis);
	jointRightPelvis->Add(rightPelvis);
	rightPelvis->Add(jointRightHip);
	jointRightHip->Add(rightThigh);
	rightThigh->Add(jointRightKnee);
	jointRightKnee->Add(rightShin);
	rightShin->Add(jointRightAnkle);
	jointRightAnkle->Add(rightFoot);
	rightFoot->Add(jointRightToes);
	jointRightToes->Add(rightToes);

	// Toros to head
	pelvis->Add(jointLowerTorso);
	jointLowerTorso->Add(lowerTorso);
	lowerTorso->Add(jointTorso);
	jointTorso->Add(torso);
	torso->Add(jointChest);
	jointChest->Add(chest);
	chest->Add(jointNeck);
	jointNeck->Add(neck);
	neck->Add(jointHead);
	jointHead->Add(head);

	// left arm
	chest->Add(jointLeftChest);
	jointLeftChest->Add(leftChest);
	leftChest->Add(jointLeftShoulder);
	jointLeftShoulder->Add(leftShoulder);
	leftShoulder->Add(jointLeftUpperArm);
	jointLeftUpperArm->Add(leftUpperArm);
	leftUpperArm->Add(jointLeftElbow);
	jointLeftElbow->Add(leftForearm);
	leftForearm->Add(jointLeftWrist);
	jointLeftWrist->Add(leftHand);
	leftHand->Add(jointLeftFingers);
	jointLeftFingers->Add(leftFingers);

	// right arm
	chest->Add(jointRightChest);
	jointRightChest->Add(rightChest);
	rightChest->Add(jointRightShoulder);
	jointRightShoulder->Add(rightShoulder);
	rightShoulder->Add(jointRightUpperArm);
	jointRightUpperArm->Add(rightUpperArm);
	rightUpperArm->Add(jointRightElbow);
	jointRightElbow->Add(rightForearm);
	rightForearm->Add(jointRightWrist);
	jointRightWrist->Add(rightHand);
	rightHand->Add(jointRightFingers);
	jointRightFingers->Add(rightFingers);


}

ModelSceneNode* BipedalMecha::CreateSimpleBox(
		float left, 
		float top, 
		float right, 
		float bot,
		float front, 
		float back,

		ID3D11Device *d3dDevice,

		ShapeLibrary* ShapeLibrary,
		int TextureID,
		int ShaderID,
		ModelLibrary* ModelLibrary)
{
	if (_bipedalMechaBasicShapes == nullptr)
	{
		_bipedalMechaBasicShapes = ref new BasicShapes(d3dDevice);
	}

	Shape *box =
		_bipedalMechaBasicShapes->CreateBoxModel(
			left, top, right, bot, front, back);

	int shapeID = 
		ShapeLibrary->Add(box);

	Model* model = new Model();

	model->ShapeID = shapeID;
	model->TextureID = TextureID;
	model->ShaderID = ShaderID;

	int modelID =
		ModelLibrary->Add(model);

	ModelSceneNode* node = 
		new ModelSceneNode();

	node->ModelID = modelID;

	return node;
}